using System;
using System.Collections.Generic;
using System.Text;
using System.Threading;
using SimpleMessageQueue;
using slcl.Packets;
using System.Timers;

namespace slcl
{
    public class MovementManager
    {
        Client Client;
        Grid Grid;

        private const float HAVOK_TIMESTEP = 1.0f / 45.0f;

        #region Enums

        public enum Command
        {
            Move,
            Autopilot,
            Sit,
            Stand,
            Walk,
            Run,
            Fly,
            Land
        }

        public enum Event
        {
            LocationChanged,
            SittingOnChanged,
            Shutdown
        }


        /// <summary>
        /// Effect type used in ViewerEffect packets
        /// </summary>
        public enum EffectType : byte
        {
            /// <summary>Place floating text above an object</summary>
            Text = 0,
            /// <summary>Unknown, probably places an icon above an object</summary>
            Icon,
            /// <summary>Unknown</summary>
            Connector,
            /// <summary>Unknown</summary>
            FlexibleObject,
            /// <summary>Unknown</summary>
            AnimalControls,
            /// <summary>Unknown</summary>
            AnimationObject,
            /// <summary>Unknown</summary>
            Cloth,
            /// <summary>Project a beam from a source to a destination, such as
            /// the one used when editing an object</summary>
            Beam,
            /// <summary>Not implemented yet</summary>
            Glow,
            /// <summary>Unknown</summary>
            Point,
            /// <summary>Unknown</summary>
            Trail,
            /// <summary>Create a swirl of particles around an object</summary>
            Sphere,
            /// <summary>Unknown</summary>
            Spiral,
            /// <summary>Unknown</summary>
            Edit,
            /// <summary>Cause an avatar to look at an object</summary>
            LookAt,
            /// <summary>Cause an avatar to point at an object</summary>
            PointAt
        }

        /// <summary>
        /// The action an avatar is doing when looking at something, used in 
        /// ViewerEffect packets for the LookAt effect
        /// </summary>
        public enum LookAtTarget : byte
        {
            /// <summary></summary>
            None,
            /// <summary></summary>
            Idle,
            /// <summary></summary>
            AutoListen,
            /// <summary></summary>
            FreeLook,
            /// <summary></summary>
            Respond,
            /// <summary></summary>
            Hover,
            /// <summary>Deprecated</summary>
            Conversation,
            /// <summary></summary>
            Select,
            /// <summary></summary>
            Focus,
            /// <summary></summary>
            Mouselook,
            /// <summary></summary>
            Clear
        }

        /// <summary>
        /// The action an avatar is doing when pointing at something, used in
        /// ViewerEffect packets for the PointAt effect
        /// </summary>
        public enum PointAtType : byte
        {
            /// <summary></summary>
            None,
            /// <summary></summary>
            Select,
            /// <summary></summary>
            Grab,
            /// <summary></summary>
            Clear
        }

        /// <summary>
        /// Used to specify movement actions for your agent
        /// </summary>
        [Flags]
        public enum AgentUpdateFlags
        {
            /// <summary>Empty flag</summary>
            NONE = 0,
            /// <summary>Move Forward (SL Keybinding: W/Up Arrow)</summary>
            AGENT_CONTROL_AT_POS = 0x1 << CONTROL_AT_POS_INDEX,
            /// <summary>Move Backward (SL Keybinding: S/Down Arrow)</summary>
            AGENT_CONTROL_AT_NEG = 0x1 << CONTROL_AT_NEG_INDEX,
            /// <summary>Move Left (SL Keybinding: Shift-(A/Left Arrow))</summary>
            AGENT_CONTROL_LEFT_POS = 0x1 << CONTROL_LEFT_POS_INDEX,
            /// <summary>Move Right (SL Keybinding: Shift-(D/Right Arrow))</summary>
            AGENT_CONTROL_LEFT_NEG = 0x1 << CONTROL_LEFT_NEG_INDEX,
            /// <summary>Not Flying: Jump/Flying: Move Up (SL Keybinding: E)</summary>
            AGENT_CONTROL_UP_POS = 0x1 << CONTROL_UP_POS_INDEX,
            /// <summary>Not Flying: Croutch/Flying: Move Down (SL Keybinding: C)</summary>
            AGENT_CONTROL_UP_NEG = 0x1 << CONTROL_UP_NEG_INDEX,
            /// <summary>Unused</summary>
            AGENT_CONTROL_PITCH_POS = 0x1 << CONTROL_PITCH_POS_INDEX,
            /// <summary>Unused</summary>
            AGENT_CONTROL_PITCH_NEG = 0x1 << CONTROL_PITCH_NEG_INDEX,
            /// <summary>Unused</summary>
            AGENT_CONTROL_YAW_POS = 0x1 << CONTROL_YAW_POS_INDEX,
            /// <summary>Unused</summary>
            AGENT_CONTROL_YAW_NEG = 0x1 << CONTROL_YAW_NEG_INDEX,
            /// <summary>ORed with AGENT_CONTROL_AT_* if the keyboard is being used</summary>
            AGENT_CONTROL_FAST_AT = 0x1 << CONTROL_FAST_AT_INDEX,
            /// <summary>ORed with AGENT_CONTROL_LEFT_* if the keyboard is being used</summary>
            AGENT_CONTROL_FAST_LEFT = 0x1 << CONTROL_FAST_LEFT_INDEX,
            /// <summary>ORed with AGENT_CONTROL_UP_* if the keyboard is being used</summary>
            AGENT_CONTROL_FAST_UP = 0x1 << CONTROL_FAST_UP_INDEX,
            /// <summary>Fly</summary>
            AGENT_CONTROL_FLY = 0x1 << CONTROL_FLY_INDEX,
            /// <summary></summary>
            AGENT_CONTROL_STOP = 0x1 << CONTROL_STOP_INDEX,
            /// <summary>Finish our current animation</summary>
            AGENT_CONTROL_FINISH_ANIM = 0x1 << CONTROL_FINISH_ANIM_INDEX,
            /// <summary>Stand up from the ground or a prim seat</summary>
            AGENT_CONTROL_STAND_UP = 0x1 << CONTROL_STAND_UP_INDEX,
            /// <summary>Sit on the ground at our current location</summary>
            AGENT_CONTROL_SIT_ON_GROUND = 0x1 << CONTROL_SIT_ON_GROUND_INDEX,
            /// <summary>Whether mouselook is currently enabled</summary>
            AGENT_CONTROL_MOUSELOOK = 0x1 << CONTROL_MOUSELOOK_INDEX,
            /// <summary>Legacy, used if a key was pressed for less than a certain amount of time</summary>
            AGENT_CONTROL_NUDGE_AT_POS = 0x1 << CONTROL_NUDGE_AT_POS_INDEX,
            /// <summary>Legacy, used if a key was pressed for less than a certain amount of time</summary>
            AGENT_CONTROL_NUDGE_AT_NEG = 0x1 << CONTROL_NUDGE_AT_NEG_INDEX,
            /// <summary>Legacy, used if a key was pressed for less than a certain amount of time</summary>
            AGENT_CONTROL_NUDGE_LEFT_POS = 0x1 << CONTROL_NUDGE_LEFT_POS_INDEX,
            /// <summary>Legacy, used if a key was pressed for less than a certain amount of time</summary>
            AGENT_CONTROL_NUDGE_LEFT_NEG = 0x1 << CONTROL_NUDGE_LEFT_NEG_INDEX,
            /// <summary>Legacy, used if a key was pressed for less than a certain amount of time</summary>
            AGENT_CONTROL_NUDGE_UP_POS = 0x1 << CONTROL_NUDGE_UP_POS_INDEX,
            /// <summary>Legacy, used if a key was pressed for less than a certain amount of time</summary>
            AGENT_CONTROL_NUDGE_UP_NEG = 0x1 << CONTROL_NUDGE_UP_NEG_INDEX,
            /// <summary></summary>
            AGENT_CONTROL_TURN_LEFT = 0x1 << CONTROL_TURN_LEFT_INDEX,
            /// <summary></summary>
            AGENT_CONTROL_TURN_RIGHT = 0x1 << CONTROL_TURN_RIGHT_INDEX,
            /// <summary>Set when the avatar is idled or set to away. Note that the away animation is 
            /// activated separately from setting this flag</summary>
            AGENT_CONTROL_AWAY = 0x1 << CONTROL_AWAY_INDEX,
            /// <summary></summary>
            AGENT_CONTROL_LBUTTON_DOWN = 0x1 << CONTROL_LBUTTON_DOWN_INDEX,
            /// <summary></summary>
            AGENT_CONTROL_LBUTTON_UP = 0x1 << CONTROL_LBUTTON_UP_INDEX,
            /// <summary></summary>
            AGENT_CONTROL_ML_LBUTTON_DOWN = 0x1 << CONTROL_ML_LBUTTON_DOWN_INDEX,
            /// <summary></summary>
            AGENT_CONTROL_ML_LBUTTON_UP = 0x1 << CONTROL_ML_LBUTTON_UP_INDEX
        }


        #endregion

        #region AgentUpdate Constants

        private const int CONTROL_AT_POS_INDEX = 0;
        private const int CONTROL_AT_NEG_INDEX = 1;
        private const int CONTROL_LEFT_POS_INDEX = 2;
        private const int CONTROL_LEFT_NEG_INDEX = 3;
        private const int CONTROL_UP_POS_INDEX = 4;
        private const int CONTROL_UP_NEG_INDEX = 5;
        private const int CONTROL_PITCH_POS_INDEX = 6;
        private const int CONTROL_PITCH_NEG_INDEX = 7;
        private const int CONTROL_YAW_POS_INDEX = 8;
        private const int CONTROL_YAW_NEG_INDEX = 9;
        private const int CONTROL_FAST_AT_INDEX = 10;
        private const int CONTROL_FAST_LEFT_INDEX = 11;
        private const int CONTROL_FAST_UP_INDEX = 12;
        private const int CONTROL_FLY_INDEX = 13;
        private const int CONTROL_STOP_INDEX = 14;
        private const int CONTROL_FINISH_ANIM_INDEX = 15;
        private const int CONTROL_STAND_UP_INDEX = 16;
        private const int CONTROL_SIT_ON_GROUND_INDEX = 17;
        private const int CONTROL_MOUSELOOK_INDEX = 18;
        private const int CONTROL_NUDGE_AT_POS_INDEX = 19;
        private const int CONTROL_NUDGE_AT_NEG_INDEX = 20;
        private const int CONTROL_NUDGE_LEFT_POS_INDEX = 21;
        private const int CONTROL_NUDGE_LEFT_NEG_INDEX = 22;
        private const int CONTROL_NUDGE_UP_POS_INDEX = 23;
        private const int CONTROL_NUDGE_UP_NEG_INDEX = 24;
        private const int CONTROL_TURN_LEFT_INDEX = 25;
        private const int CONTROL_TURN_RIGHT_INDEX = 26;
        private const int CONTROL_AWAY_INDEX = 27;
        private const int CONTROL_LBUTTON_DOWN_INDEX = 28;
        private const int CONTROL_LBUTTON_UP_INDEX = 29;
        private const int CONTROL_ML_LBUTTON_DOWN_INDEX = 30;
        private const int CONTROL_ML_LBUTTON_UP_INDEX = 31;
        private const int TOTAL_CONTROLS = 32;

        #endregion AgentUpdate Constants

        /// <summary>
        /// Contains properties for client control key states
        /// These are the values that you "set" in order to cause your avatar to take action.
        /// </summary>
        public struct ControlStatus
        {
            /// <summary></summary>
            public bool AtPos
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_AT_POS); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_AT_POS, value); }
            }
            /// <summary></summary>
            public bool AtNeg
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_AT_NEG); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_AT_NEG, value); }
            }
            /// <summary></summary>
            public bool LeftPos
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_LEFT_POS); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_LEFT_POS, value); }
            }
            /// <summary></summary>
            public bool LeftNeg
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_LEFT_NEG); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_LEFT_NEG, value); }
            }
            /// <summary></summary>
            public bool UpPos
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_UP_POS); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_UP_POS, value); }
            }
            /// <summary></summary>
            public bool UpNeg
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_UP_NEG); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_UP_NEG, value); }
            }
            /// <summary></summary>
            public bool PitchPos
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_PITCH_POS); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_PITCH_POS, value); }
            }
            /// <summary></summary>
            public bool PitchNeg
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_PITCH_NEG); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_PITCH_NEG, value); }
            }
            /// <summary></summary>
            public bool YawPos
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_YAW_POS); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_YAW_POS, value); }
            }
            /// <summary></summary>
            public bool YawNeg
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_YAW_NEG); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_YAW_NEG, value); }
            }
            /// <summary></summary>
            public bool FastAt
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_FAST_AT); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_FAST_AT, value); }
            }
            /// <summary></summary>
            public bool FastLeft
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_FAST_LEFT); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_FAST_LEFT, value); }
            }
            /// <summary></summary>
            public bool FastUp
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_FAST_UP); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_FAST_UP, value); }
            }
            /// <summary></summary>
            public bool Fly
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_FLY); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_FLY, value); }
            }
            /// <summary></summary>
            public bool Stop
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_STOP); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_STOP, value); }
            }
            /// <summary></summary>
            public bool FinishAnim
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_FINISH_ANIM); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_FINISH_ANIM, value); }
            }
            /// <summary></summary>
            public bool StandUp
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_STAND_UP); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_STAND_UP, value); }
            }
            /// <summary></summary>
            public bool SitOnGround
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_SIT_ON_GROUND); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_SIT_ON_GROUND, value); }
            }
            /// <summary></summary>
            public bool Mouselook
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_MOUSELOOK); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_MOUSELOOK, value); }
            }
            /// <summary></summary>
            public bool NudgeAtPos
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_NUDGE_AT_POS); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_NUDGE_AT_POS, value); }
            }
            /// <summary></summary>
            public bool NudgeAtNeg
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_NUDGE_AT_NEG); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_NUDGE_AT_NEG, value); }
            }
            /// <summary></summary>
            public bool NudgeLeftPos
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_NUDGE_LEFT_POS); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_NUDGE_LEFT_POS, value); }
            }
            /// <summary></summary>
            public bool NudgeLeftNeg
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_NUDGE_LEFT_NEG); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_NUDGE_LEFT_NEG, value); }
            }
            /// <summary></summary>
            public bool NudgeUpPos
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_NUDGE_UP_POS); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_NUDGE_UP_POS, value); }
            }
            /// <summary></summary>
            public bool NudgeUpNeg
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_NUDGE_UP_NEG); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_NUDGE_UP_NEG, value); }
            }
            /// <summary></summary>
            public bool TurnLeft
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_TURN_LEFT); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_TURN_LEFT, value); }
            }
            /// <summary></summary>
            public bool TurnRight
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_TURN_RIGHT); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_TURN_RIGHT, value); }
            }
            /// <summary></summary>
            public bool Away
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_AWAY); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_AWAY, value); }
            }
            /// <summary></summary>
            public bool LButtonDown
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_LBUTTON_DOWN); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_LBUTTON_DOWN, value); }
            }
            /// <summary></summary>
            public bool LButtonUp
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_LBUTTON_UP); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_LBUTTON_UP, value); }
            }
            /// <summary></summary>
            public bool MLButtonDown
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_ML_LBUTTON_DOWN); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_ML_LBUTTON_DOWN, value); }
            }
            /// <summary></summary>
            public bool MLButtonUp
            {
                get { return GetControlFlag(AgentUpdateFlags.AGENT_CONTROL_ML_LBUTTON_UP); }
                set { SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_ML_LBUTTON_UP, value); }
            }
        }

        /// <summary>
        /// Where the camera is pointing to and from.
        /// </summary>
        public struct CameraStatus
        {
            /// <summary></summary>
            public LLQuaternion BodyRotation;
            /// <summary></summary>
            public LLQuaternion HeadRotation;
            /// <summary></summary>
            public LLVector3 CameraAtAxis;
            /// <summary></summary>
            public LLVector3 CameraCenter;
            /// <summary></summary>
            public LLVector3 CameraLeftAxis;
            /// <summary></summary>
            public LLVector3 CameraUpAxis;
            /// <summary></summary>
            public float Far;
        }


        /// <summary>
        /// Timer for sending AgentUpdate packets, disabled by default
        /// </summary>
        public System.Timers.Timer UpdateTimer;

        /// <summary>
        /// Holds control flags
        /// </summary>
        public ControlStatus Controls;

        /// <summary>
        /// Holds camera flags
        /// </summary>
        public CameraStatus Camera;

        /// <summary>The point the avatar is currently looking at
        /// (may not stay updated)</summary>
        public LLVector3 LookAt = LLVector3.Zero;

        /// <summary>Current position of avatar</summary>
        public LLVector3 Position = LLVector3.Zero;
        /// <summary>Current rotation of avatar</summary>
        public LLQuaternion Rotation = LLQuaternion.Identity;

        /// <summary>Gets the local ID of the prim the avatar is sitting on,
        /// zero if the avatar is not currently sitting</summary>
        public uint SittingOn { get { return sittingOn; } }
        internal uint sittingOn = 0;

        /// <summary></summary>
        public LLVector4 CollisionPlane = LLVector4.Zero;
        /// <summary></summary>
        public LLVector3 Velocity = LLVector3.Zero;
        /// <summary></summary>
        public LLVector3 Acceleration = LLVector3.Zero;
        /// <summary></summary>
        public LLVector3 AngularVelocity = LLVector3.Zero;

        DateTime lastInterpolation;

        /// <summary>
        /// Returns "always run" value, or changes it by sending a SetAlwaysRunPacket
        /// </summary>
        public bool AlwaysRun
        {
            get
            {
                return alwaysRun;
            }
            set
            {
                alwaysRun = value;
                SetAlwaysRunPacket run = new SetAlwaysRunPacket();
                run.AgentData.AgentID = Client.Grid.AgentID;
                run.AgentData.SessionID = Client.Grid.SessionID;
                run.AgentData.AlwaysRun = alwaysRun;
                Client.Grid.SendPacket(run);
            }
        }


        /// <summary>
        /// All movements are to be done in the "run" mode
        /// </summary>
        private bool alwaysRun = false;

        /// <summary>
        /// bitmap of our curent "movement" and "state".
        /// </summary>
        private static uint agentControls;

        /// <summary>The current value of the agent control flags</summary>
        public uint AgentControls { get { return agentControls; } }

        /// <summary>
        /// timer to update out location based on speed and direction.
        /// </summary>
        private System.Timers.Timer InterpolationTimer;

        MessageQueue MQueue;

        #region Sit execution vars

        /// <summary>
        /// the object that we are attempting to sit on.
        /// </summary>
        LLUUID WaitingForSitOn;
        /// <summary>
        /// get's set when our sittingOn is changed.
        /// </summary>
        ManualResetEvent SitChangedEvent = new ManualResetEvent(false);

        /// <summary>
        /// timeout for sit request.
        /// </summary>
        System.Timers.Timer SitTimeoutTimer = new System.Timers.Timer(10000);
        /// <summary>
        /// get's filled with the command that is asking us to sit.
        /// </summary>
        CommandMessage SitCommand = null;

        #endregion
        
        public MovementManager(Client client)
        {
            Client = client;
            Grid = Client.Grid;
            MQueue = new MessageQueue(this, true);
            MQueue.RegisterPacketCallback(Message.MessageType.Command, new MessageQueue.MessageCallback(HandleCommand));
            // initialize our head camera
            Camera.BodyRotation = LLQuaternion.Identity;
            Camera.HeadRotation = LLQuaternion.Identity;
            Camera.CameraCenter = new LLVector3(128, 128, 20);
            Camera.CameraAtAxis = new LLVector3(0, 0.9999f, 0);
            Camera.CameraLeftAxis = new LLVector3(0.9999f, 0, 0);
            Camera.CameraUpAxis = new LLVector3(0, 0, 0.9999f);
            Camera.Far = 128.0f;

            // updates timer. This sends a packet out to the sim telling it about our curent movement state.

            UpdateTimer = new System.Timers.Timer(Client.Settings.AGENT_UPDATE_INTERVAL);
            UpdateTimer.Elapsed += new ElapsedEventHandler(UpdateTimer_Elapsed);
            UpdateTimer.Enabled = Client.Settings.SEND_AGENT_UPDATES;

            // Movement complete callback
            MQueue.Send(new CommandMessage(Grid.Command.Register, PacketType.AgentMovementComplete
                , new CommandMessage.AckCallback(MovementCompleteHandler)), Grid);
            SitTimeoutTimer.Elapsed += new ElapsedEventHandler(SitTimeoutTimer_Elapsed);
            InterpolationTimer = new System.Timers.Timer(Settings.INTERPOLATION_UPDATE);
            InterpolationTimer.Elapsed += new System.Timers.ElapsedEventHandler(InterpolationTimer_Elapsed);
            InterpolationTimer.Start();
        }

        public void HandleCommand(Message cmd)
        {
            CommandMessage Cmd = (CommandMessage)cmd;
            switch ((Command)Cmd.Command)
            {
                case Command.Autopilot:
                    break;
                case Command.Move:
                    break;
                case Command.Sit:
                    object[] args = (object[])Cmd.Data;
                    SitCommand = Cmd;
                    Sit((LLUUID)args[0], LLVector3.Zero);
                    break;
                case Command.Stand:
                    Stand();
                    MQueue.Ack(Cmd, AckMessage.StatusType.Completed);
                    break;
                case Command.Walk:
                    AlwaysRun = false;
                    break;
                case Command.Run:
                    AlwaysRun = true;
                    break;
                case Command.Fly:                    
                    break;
                case Command.Land:
                    break;
            }
        }

        void SitTimeoutTimer_Elapsed(object sender, ElapsedEventArgs e)
        {
            if (null != SitCommand)
            {
                MQueue.Ack(SitCommand, AckMessage.StatusType.Failed, "Timer elapsed.");
                SitCommand = null;
                SitTimeoutTimer.Stop();
            }
        }



        #region Client Requests
		
        /// <summary>
        /// Sends a request to sit on the specified object
        /// </summary>
        /// <param name="targetID">LLUUID of the object to sit on</param>
        /// <param name="offset">Sit at offset</param>
        public void RequestSit(LLUUID targetID, LLVector3 offset)
        {
            AgentRequestSitPacket requestSit = new AgentRequestSitPacket();
            requestSit.AgentData.AgentID = Grid.AgentID;
            requestSit.AgentData.SessionID = Grid.SessionID;
            requestSit.TargetObject.TargetID = targetID;
            requestSit.TargetObject.Offset = offset;
            Grid.SendPacket(requestSit);
        }

        /// <summary>
        /// Follows a call to RequestSit() to actually sit on the object
        /// </summary>
        public void SendSit()
        {
            AgentSitPacket sit = new AgentSitPacket();
            sit.AgentData.AgentID = Grid.AgentID;
            sit.AgentData.SessionID = Grid.SessionID;
            Grid.SendPacket(sit);
        }


	    #endregion 
        
        #region Packet Handlers

        /// <summary>
        /// called by ObjectManager since SL treats our avie as just another object out there. *sigh*
        /// </summary>
        /// <param name="blockID"></param>
        /// <param name="collisionPlane"></param>
        /// <param name="position"></param>
        /// <param name="velocity"></param>
        /// <param name="acceleration"></param>
        /// <param name="rotation"></param>
        /// <param name="angularVelocity"></param>
        /// <param name="ParentID"></param>
        public void UpdateLocation(uint blockID, LLVector4 collisionPlane, LLVector3 position, LLVector3 velocity
            , LLVector3 acceleration, LLQuaternion rotation, LLVector3 angularVelocity, uint ParentID)
        {
            Client.LocalID = blockID;
            // Packed parameters
            Client.Movement.CollisionPlane = collisionPlane;
            Client.Movement.Position = position;
            Client.Movement.Velocity = velocity;
            Client.Movement.Acceleration = acceleration;
            Client.Movement.Rotation = rotation;
            Client.Movement.AngularVelocity = angularVelocity;

            // Detect if we are sitting or standing
            uint oldSittingOn = sittingOn;

            // Fire the callback for our sitting orientation changing
            if (ParentID != oldSittingOn)
            {
                sittingOn = ParentID;
                SitChangedEvent.Set();
                if (null != SitCommand)
                {
                    MQueue.Ack(SitCommand, AckMessage.StatusType.Completed, sittingOn);
                    SitCommand = null;
                    SitTimeoutTimer.Stop();
                }
                MQueue.MatchRegistered(Event.SittingOnChanged, AckMessage.StatusType.Completed, sittingOn);
            }
            MQueue.MatchRegistered(Event.LocationChanged, AckMessage.StatusType.Completed
                , new object[] { collisionPlane, position, velocity, acceleration, rotation
                    , angularVelocity, sittingOn });
        }

        /// <summary>
        /// Handler for teleport Requests
        /// </summary>

        void MovementCompleteHandler(AckMessage Ack)
        {
            PacketCarrier carrier = (PacketCarrier)Ack.Data;
            AgentMovementCompletePacket movement = (AgentMovementCompletePacket)carrier.packet;

            this.Position = movement.Data.Position;
            this.LookAt = movement.Data.LookAt;
            MQueue.MatchRegistered(Event.LocationChanged, AckMessage.StatusType.Completed, Position);
        }


        #endregion


        /// <summary>
        /// Send new AgentUpdate packet to update our current camera 
        /// position and rotation
        /// </summary>
        public void SendUpdate()
        {
            SendUpdate(false, Client.Grid.Sim);
        }

        /// <summary>
        /// Send new AgentUpdate packet to update our current camera 
        /// position and rotation
        /// </summary>
        /// <param name="reliable">Whether to require server acknowledgement
        /// of this packet</param>
        public void SendUpdate(bool reliable)
        {
            SendUpdate(reliable, Client.Grid.Sim);
        }

        /// <summary>
        /// Send new AgentUpdate packet to update our current camera 
        /// position and rotation
        /// </summary>
        /// <param name="reliable">Whether to require server acknowledgement
        /// of this packet</param>
        /// <param name="simulator">Simulator to send the update to</param>
        public void SendUpdate(bool reliable, Simulator simulator)
        {
            AgentUpdatePacket update = new AgentUpdatePacket();
            update.Header.Reliable = reliable;

            update.AgentData.AgentID = Client.Grid.AgentID;
            update.AgentData.SessionID = Client.Grid.SessionID;
            update.AgentData.HeadRotation = Camera.HeadRotation;
            update.AgentData.BodyRotation = Camera.BodyRotation;
            update.AgentData.CameraAtAxis = Camera.CameraAtAxis;
            update.AgentData.CameraCenter = Camera.CameraCenter;
            update.AgentData.CameraLeftAxis = Camera.CameraLeftAxis;
            update.AgentData.CameraUpAxis = Camera.CameraUpAxis;
            update.AgentData.ControlFlags = agentControls;
            update.AgentData.Far = Camera.Far;

            simulator.SendPacket(update);
        }

        private static bool GetControlFlag(AgentUpdateFlags flag)
        {
            uint control = (uint)flag;
            return ((agentControls & control) == control);
        }

        private static void SetControlFlag(AgentUpdateFlags flag, bool value)
        {
            uint control = (uint)flag;
            if (value && ((agentControls & control) != control)) agentControls ^= control;
            else if (!value && ((agentControls & control) == control)) agentControls ^= control;
        }

        private void UpdateTimer_Elapsed(object sender, ElapsedEventArgs e)
        {
            if (Client.Grid.Connected)
            {
                //Send an AgentUpdate packet
                SendUpdate();
            }
        }


        #region Client Actions to perform

        public void Sit(LLUUID id, LLVector3 offset)
        {
            SitChangedEvent.Reset();
            RequestSit(id, offset);
            SendSit();
            SitTimeoutTimer.Start();
        }

        public void Stand()
        {
            SetControlFlag(AgentUpdateFlags.AGENT_CONTROL_STAND_UP, true);
            SendUpdate();
        }

        /// <summary>
        /// Use the autopilot sim function to move the avatar to a new position
        /// </summary>
        /// <remarks>The z value is currently not handled properly by the simulator</remarks>
        /// <param name="globalX">Integer value for the global X coordinate to move to</param>
        /// <param name="globalY">Integer value for the global Y coordinate to move to</param>
        /// <param name="z">Floating-point value for the Z coordinate to move to</param>
        /// <example>AutoPilot(252620, 247078, 20.2674);</example>
        public void AutoPilot(ulong globalX, ulong globalY, float z)
        {
            GenericMessagePacket autopilot = new GenericMessagePacket();

            autopilot.AgentData.AgentID = Grid.AgentID;
            autopilot.AgentData.SessionID = Grid.SessionID;
            autopilot.AgentData.TransactionID = LLUUID.Zero;
            autopilot.MethodData.Invoice = LLUUID.Zero;
            autopilot.MethodData.Method = Helpers.StringToField("autopilot");
            autopilot.ParamList = new GenericMessagePacket.ParamListBlock[3];
            autopilot.ParamList[0] = new GenericMessagePacket.ParamListBlock();
            autopilot.ParamList[0].Parameter = Helpers.StringToField(globalX.ToString());
            autopilot.ParamList[1] = new GenericMessagePacket.ParamListBlock();
            autopilot.ParamList[1].Parameter = Helpers.StringToField(globalY.ToString());
            autopilot.ParamList[2] = new GenericMessagePacket.ParamListBlock();
            // TODO: Do we need to prevent z coordinates from being sent in 1.4827e-18 notation?
            autopilot.ParamList[2].Parameter = Helpers.StringToField(z.ToString());

            Grid.SendPacket(autopilot);
        }

        /// <summary>
        /// Use the autopilot sim function to move the avatar to a new position
        /// </summary>
        /// <remarks>The z value is currently not handled properly by the simulator</remarks>
        /// <param name="localX">Integer value for the local X coordinate to move to</param>
        /// <param name="localY">Integer value for the local Y coordinate to move to</param>
        /// <param name="z">Floating-point value for the Z coordinate to move to</param>
        public void AutoPilotLocal(int localX, int localY, float z)
        {
            uint x, y;
            Helpers.LongToUInts(Grid.Sim.Handle, out x, out y);
            AutoPilot((ulong)(x + localX), (ulong)(y + localY), z);
        }


        /// <summary>
        /// Grabs an object
        /// </summary>
        /// <param name="objectLocalID">Local ID of Object to grab</param>
        public void Grab(uint objectLocalID)
        {
            ObjectGrabPacket grab = new ObjectGrabPacket();
            grab.AgentData.AgentID = Grid.AgentID;
            grab.AgentData.SessionID = Grid.SessionID;
            grab.ObjectData.LocalID = objectLocalID;
            grab.ObjectData.GrabOffset = new LLVector3(0, 0, 0);
            Grid.SendPacket(grab);
        }

        /// <summary>
        /// Drags on an object
        /// </summary>
        /// <param name="objectID">Strangely, LLUID instead of local ID</param>
        /// <param name="grabPosition">Drag target in region coordinates</param>
        public void GrabUpdate(LLUUID objectID, LLVector3 grabPosition)
        {
            ObjectGrabUpdatePacket grab = new ObjectGrabUpdatePacket();
            grab.AgentData.AgentID = Grid.AgentID;
            grab.AgentData.SessionID = Grid.SessionID;
            grab.ObjectData.ObjectID = objectID;
            grab.ObjectData.GrabOffsetInitial = new LLVector3(0, 0, 0);
            grab.ObjectData.GrabPosition = grabPosition;
            grab.ObjectData.TimeSinceLast = 0;
            Grid.SendPacket(grab);
        }

        /// <summary>
        /// Releases a grabbed object
        /// </summary>
        public void DeGrab(uint objectLocalID)
        {
            ObjectDeGrabPacket degrab = new ObjectDeGrabPacket();
            degrab.AgentData.AgentID = Grid.AgentID;
            degrab.AgentData.SessionID = Grid.SessionID;
            degrab.ObjectData.LocalID = objectLocalID;
            Grid.SendPacket(degrab);
        }

        /// <summary>
        /// Touches an object
        /// </summary>
        public void Touch(uint objectLocalID)
        {
            Grab(objectLocalID);
            DeGrab(objectLocalID);
        }


        /// <summary>
        /// Sends camera and action updates to the server including the 
        /// position and orientation of our camera, and a ControlFlags field
        /// specifying our current movement actions
        /// </summary>
        /// <param name="controlFlags"></param>
        /// <param name="position"></param>
        /// <param name="forwardAxis"></param>
        /// <param name="leftAxis"></param>
        /// <param name="upAxis"></param>
        /// <param name="bodyRotation"></param>
        /// <param name="headRotation"></param>
        /// <param name="farClip"></param>
        /// <param name="reliable"></param>
        public void UpdateCamera(AgentUpdateFlags controlFlags, LLVector3 position, LLVector3 forwardAxis,
            LLVector3 leftAxis, LLVector3 upAxis, LLQuaternion bodyRotation, LLQuaternion headRotation, float farClip,
            bool reliable)
        {
            AgentUpdatePacket update = new AgentUpdatePacket();

            update.AgentData.AgentID = Grid.AgentID;
            update.AgentData.SessionID = Grid.SessionID;
            update.AgentData.State = 0;
            update.AgentData.BodyRotation = bodyRotation;
            update.AgentData.HeadRotation = headRotation;
            update.AgentData.CameraCenter = position;
            update.AgentData.CameraAtAxis = forwardAxis;
            update.AgentData.CameraLeftAxis = leftAxis;
            update.AgentData.CameraUpAxis = upAxis;
            update.AgentData.Far = farClip;
            update.AgentData.ControlFlags = (uint)controlFlags;
            update.AgentData.Flags = 0;
            update.Header.Reliable = reliable;

            Grid.SendPacket(update);
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="sourceAvatar"></param>
        /// <param name="targetObject"></param>
        /// <param name="globalOffset"></param>
        /// <param name="type"></param>
        public void PointAtEffect(LLUUID sourceAvatar, LLUUID targetObject, LLVector3d globalOffset, PointAtType type)
        {
            ViewerEffectPacket effect = new ViewerEffectPacket();

            effect.AgentData.AgentID = Grid.AgentID;
            effect.AgentData.SessionID = Grid.SessionID;

            effect.Effect = new ViewerEffectPacket.EffectBlock[1];
            effect.Effect[0] = new ViewerEffectPacket.EffectBlock();
            effect.Effect[0].AgentID = Grid.AgentID;
            effect.Effect[0].Color = LLColor.Black.GetBytes();
            effect.Effect[0].Duration = (type == PointAtType.Clear) ? 0.0f : Single.MaxValue / 4.0f;
            effect.Effect[0].ID = LLUUID.Random();
            effect.Effect[0].Type = (byte)EffectType.PointAt;

            byte[] typeData = new byte[57];
            if (sourceAvatar != null)
                Array.Copy(sourceAvatar.GetBytes(), typeData, 16);
            if (targetObject != null)
                Array.Copy(targetObject.GetBytes(), 0, typeData, 16, 16);
            Array.Copy(globalOffset.GetBytes(), 0, typeData, 32, 24);
            typeData[56] = (byte)type;

            effect.Effect[0].TypeData = typeData;

            Grid.SendPacket(effect);
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="sourceAvatar"></param>
        /// <param name="targetObject"></param>
        /// <param name="globalOffset"></param>
        /// <param name="type"></param>
        public void LookAtEffect(LLUUID sourceAvatar, LLUUID targetObject, LLVector3d globalOffset, LookAtTarget type)
        {
            ViewerEffectPacket effect = new ViewerEffectPacket();

            effect.AgentData.AgentID = Grid.AgentID;
            effect.AgentData.SessionID = Grid.SessionID;

            float duration;

            switch (type)
            {
                case LookAtTarget.Clear:
                    duration = 0.0f;
                    break;
                case LookAtTarget.Hover:
                    duration = 1.0f;
                    break;
                case LookAtTarget.FreeLook:
                    duration = 2.0f;
                    break;
                case LookAtTarget.Idle:
                    duration = 3.0f;
                    break;
                case LookAtTarget.AutoListen:
                case LookAtTarget.Respond:
                    duration = 4.0f;
                    break;
                case LookAtTarget.None:
                case LookAtTarget.Conversation:
                case LookAtTarget.Select:
                case LookAtTarget.Focus:
                case LookAtTarget.Mouselook:
                    duration = Single.MaxValue / 2.0f;
                    break;
                default:
                    duration = 0.0f;
                    break;
            }

            effect.Effect = new ViewerEffectPacket.EffectBlock[1];
            effect.Effect[0] = new ViewerEffectPacket.EffectBlock();
            effect.Effect[0].AgentID = Grid.AgentID;
            effect.Effect[0].Color = LLColor.Black.GetBytes();
            effect.Effect[0].Duration = duration;
            effect.Effect[0].ID = LLUUID.Random();
            effect.Effect[0].Type = (byte)EffectType.LookAt;

            byte[] typeData = new byte[57];
            if (sourceAvatar != null)
                Array.Copy(sourceAvatar.GetBytes(), typeData, 16);
            if (targetObject != null)
                Array.Copy(targetObject.GetBytes(), 0, typeData, 16, 16);
            typeData[56] = (byte)type;

            effect.Effect[0].TypeData = typeData;

            Grid.SendPacket(effect);
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="sourceAvatar"></param>
        /// <param name="targetObject"></param>
        /// <param name="globalOffset"></param>
        /// <param name="color"></param>
        /// <param name="duration"></param>
        public void BeamEffect(LLUUID sourceAvatar, LLUUID targetObject, LLVector3d globalOffset, LLColor color,
            float duration)
        {
            ViewerEffectPacket effect = new ViewerEffectPacket();

            effect.AgentData.AgentID = Grid.AgentID;
            effect.AgentData.SessionID = Grid.SessionID;

            effect.Effect = new ViewerEffectPacket.EffectBlock[1];
            effect.Effect[0] = new ViewerEffectPacket.EffectBlock();
            effect.Effect[0].AgentID = Grid.AgentID;
            effect.Effect[0].Color = color.GetBytes();
            effect.Effect[0].Duration = duration;
            effect.Effect[0].ID = LLUUID.Random();
            effect.Effect[0].Type = (byte)EffectType.Beam;

            byte[] typeData = new byte[56];
            if (sourceAvatar != null)
                Array.Copy(sourceAvatar.GetBytes(), 0, typeData, 0, 16);
            if (targetObject != null)
                Array.Copy(targetObject.GetBytes(), 0, typeData, 16, 16);
            Array.Copy(globalOffset.GetBytes(), 0, typeData, 32, 24);

            effect.Effect[0].TypeData = typeData;

            Grid.SendPacket(effect);
        }

        /// <summary>
        /// Send an AgentAnimation packet that toggles a single animation on
        /// </summary>
        /// <param name="animation">The animation to start playing</param>
        public void AnimationStart(LLUUID animation)
        {
            Dictionary<LLUUID, bool> animations = new Dictionary<LLUUID, bool>();
            animations[animation] = true;

            Animate(animations);
        }

        /// <summary>
        /// Send an AgentAnimation packet that toggles a single animation off
        /// </summary>
        /// <param name="animation">The animation to stop playing</param>
        public void AnimationStop(LLUUID animation)
        {
            Dictionary<LLUUID, bool> animations = new Dictionary<LLUUID, bool>();
            animations[animation] = false;

            Animate(animations);
        }

        /// <summary>
        /// Send an AgentAnimation packet that will toggle animations on or off
        /// </summary>
        /// <param name="animations">A list of animation UUIDs, and whether to
        /// turn that animation on or off</param>
        public void Animate(Dictionary<LLUUID, bool> animations)
        {
            AgentAnimationPacket animate = new AgentAnimationPacket();

            animate.AgentData.AgentID = Grid.AgentID;
            animate.AgentData.SessionID = Grid.SessionID;
            animate.AnimationList = new AgentAnimationPacket.AnimationListBlock[animations.Count];
            int i = 0;

            foreach (KeyValuePair<LLUUID, bool> animation in animations)
            {
                animate.AnimationList[i] = new AgentAnimationPacket.AnimationListBlock();
                animate.AnimationList[i].AnimID = animation.Key;
                animate.AnimationList[i].StartAnim = animation.Value;

                i++;
            }

            Grid.SendPacket(animate);
        }

        protected void InterpolationTimer_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            if (Client.Grid.Connected)
            {
                // For now we only update Client.Self since there is no object tracking
                // built in to the library

                // Only do movement interpolation (extrapolation) when there is a non-zero velocity but 
                // no acceleration
                if (Velocity != LLVector3.Zero || Acceleration != LLVector3.Zero)
                {
                    try
                    {
                        TimeSpan interval = DateTime.Now - lastInterpolation;
                        float adjSeconds = (float)interval.TotalSeconds * Client.Grid.Sim.Dilation;

                        Position += (Velocity + (0.5f * (adjSeconds - HAVOK_TIMESTEP)) *
                            Acceleration) * adjSeconds;
                    }
                    catch (Exception except)
                    {
                        Client.Log(except.ToString(), LogLevel.Warning);
                    }
                }
            }

            // Make sure the last interpolated time is always updated
            lastInterpolation = DateTime.Now;
        }

        #endregion
    }
}
